#include <bfl/wrappers/matrix/matrix_wrapper.h>
#include "AltitudeDataFusion4Test.h"

using namespace std;
using namespace ekf;

/*================================================================================================================
Note:  使用此test_AltitudeDataFusion请在安装好以来后，在linux下单独编译或放在Ros某软件包下编译。
并在CMakeList.txt文件中add_executable（）添加：
src/AltitudeDataFusion4Test.cpp
src/test_AltitudeDataFusion.cpp
//=================================================================================================================
*/

const int DATA_COUNT = 30;
float measurement_gps[DATA_COUNT] = {0, 104, 210, 290, 398, 499, 613, 694, 812, 909, 993, 1091, 1208, 1301, 1395, 1509, 1605, 1691, 1806, 1906, 1996, 2089, 2206, 2309, 2391, 2502, 2607, 2690, 2789, 2907};
float measurement_barometers_H[DATA_COUNT] = {0, 75, 155, 332, 419, 509, 567, 721, 834, 938, 993, 1101, 1204, 1274, 1438, 1504, 1564, 1742, 1811, 1934, 1961, 2117, 2192, 2259, 2403, 2464, 2556, 2708, 2782, 2920};
float measurement_barometers[DATA_COUNT] = {101.325, 100.4266099631849, 99.47544773789187, 97.39691042499965, 96.38822795303125, 95.3536816370964,
                                          94.69175182921913, 92.95225021091464, 91.69239628071, 90.5451380048492, 89.94313080200484, 88.77043762058652,
                                          87.66360020799254, 86.91777911154804, 85.19053188412288, 84.5033128758858, 83.88247028293578, 82.06233465092055,
                                          81.36543736747255, 80.13503058455179, 79.8669686844874, 78.33234584516421, 77.6030945763177, 76.95628843940422,
                                          75.58090547584266, 75.00431317586846, 74.14143986112667, 72.73344519196905, 72.05585485049728, 70.80589611085179};
float measurement_imu[DATA_COUNT] = {5, 109, 191, 276, 428, 490, 594, 678, 774, 925, 1000, 1086, 1173, 1321, 1386, 1488, 1613, 1702, 1806, 1878, 1977, 2094, 2181, 2277, 2375, 2505, 2611, 2697, 2790, 2872};
float stateXArray[DATA_COUNT];

/*
Note:  此案例仅仅依赖输入的模拟数据,产生模拟结果,无图形化界面显示.
需要查看图形曲线,请到目录下ekf_AltitudeDataFusion.xlsx查看

# 下面是某次运行本程序的输出结果
R = [[1.e-06 0.e+00 0.e+00]
    [0.e+00 1.e-02 0.e+00]
    [0.e+00 0.e+00 1.e-03]]

#基于 measurement_barometers:
stateXArray =[0.14953209537134615, 95.46584023034309, 200.29502957437685, 282.40686231986575, 388.65192735778544, 489.7344090308052,
               602.568686577119, 686.2450890345611, 801.189098995206, 900.1725172278325, 985.3377062894409, 1082.1607189648387,
               1197.2093484085533, 1292.5434152487187, 1386.3912160828295, 1498.6052924229382, 1596.2178206182107, 1683.2114311052546,
               1795.7809829820792, 1896.5728378370507, 1987.5526023065893, 2080.6023498384834, 2195.336977486254, 2299.249965958058,
               2383.218747691464, 2492.1417623942743, 2597.477309953391, 2682.3632538785755, 2780.1342709938444, 2896.1241138077853]
*/

int main(int argc, char **argv)
{
  /*
    # 模拟数据产生详见python版本:  ekf_AltitudeDataFusion.py
    */

  int state_count = 1;
  int measurement_count = 3;

  cout << "my_robot_pose_ekf              start\n";
  //Create a new Kalman filter for mouse tracking
  AltitudeDataFusion kalfilt(state_count, measurement_count, 0.01, 1e-4, 0.0001, 30);

  //更新传感器噪声矩阵,三个传感器具有不同的噪声(测量误差)
  SymmetricMatrix r_k(measurement_count); //矩阵3x3
  r_k = 0;
  r_k(1, 1) = 0.000001;  // GPS
  r_k(2, 2) = 0.01;      // 大气压测高
  r_k(3, 3) = 0.001;     // IMU 惯性传感器
  kalfilt.updateRk(r_k); // 更新传感器噪声矩阵,三个传感器具有不同的噪声(测量误差)
  cout << "my_robot_pose_ekf             R_k： " << r_k << "\n";

  ColumnVector estimateX;
  for (int index = 0; index < DATA_COUNT; index++)
  {
    ColumnVector measurementZ(measurement_count);
    measurementZ(1) = measurement_gps[index];
    measurementZ(2) = measurement_barometers[index];
    measurementZ(3) = measurement_imu[index];

    //输入当前鼠标位置测量值,  返回新的鼠标位置最优评估值
    kalfilt.doStep(measurementZ, estimateX);

    stateXArray[index] = estimateX(1);
  }
  cout << "my_robot_pose_ekf              end\n";

  cout << "Result: \nstateXArray=[";
  for (int index = 0; index < DATA_COUNT; index++)
    cout << stateXArray[index] << ",";
  cout << "]\n";

  return 0;
}
